/*****
 *
 *  sketch_servo_and_serial_com
 *
 *  2008, James Stone, http://www.manofstone.com/
 *
 *  derived from the code posted by nexisnet to this forum:
 *  http://forum.pololu.com/viewtopic.php?f=16&t=745&st=0&sk=t&sd=a&start=30
 *
 *  serial derived from  Heather Dewey-Hagborg August 15 2006  http://www.arduino.cc
 *  v0.1 - code tested, moves servos
 *  v0.2 - moved serial comm to pololu borad to pin 3
 *  v0.3 - pin 13 to reset, set to low to reset
 *  v0.4 - removed reset code, added code for vibration motor and solinoids
 *
 *  NOTE: Known issue, you have to reset the micro servo board after
 *        uploading new code to the Arduino.
 *
 *****/

#include <ctype.h>
// don't change this
#define bit9600Delay 84  
#define halfBit9600Delay 42
#define bit4800Delay 188 
#define halfBit4800Delay 94 

byte tx = 3;              // pin to commuicate to the pololu board
int vibMotor = 7;         // vibration motor, to ease cutting
int sol1 = 8;             // sol #1, for clamping paper
int sol2 = 9;             // sol #2, for clamping paper
int sol3 = 10;            // sol #3, for clamping paper
int sol4 = 11;            // sol #4, for clamping paper

byte SWval;
int incomingByte = 0;	// for incoming serial data

void setup() {
  // init homebrew serial port on pin tx, send only
  pinMode(tx,OUTPUT);
  digitalWrite(tx,HIGH);

  // setup output for motors, solenoids
  pinMode(vibMotor,OUTPUT);
  pinMode(sol1,OUTPUT);
  pinMode(sol2,OUTPUT);
  pinMode(sol3,OUTPUT);
  pinMode(sol4,OUTPUT);
  
  // init usb serial for communication to processing @ 9600 baud
  Serial.begin(9600);
 

}

void SWprint(int data)
{
  byte mask;
  //startbit
  digitalWrite(tx,LOW);
  delayMicroseconds(bit9600Delay);
  for (mask = 0x01; mask>0; mask <<= 1) {
    if (data & mask){ // choose bit
     digitalWrite(tx,HIGH); // send 1
    }
    else{
     digitalWrite(tx,LOW); // send 0
    }
    delayMicroseconds(bit9600Delay);
  }
  //stop bit
  digitalWrite(tx, HIGH);
  delayMicroseconds(bit9600Delay);
}


   
void put(int servo, int angle)
    {//servo is the servo number (typically 0-7)
    //angle is the absoltue position from 500 to 5500

       unsigned char buff[6];

       unsigned int temp;
       unsigned char pos_hi,pos_low;
       
       temp=angle&0x1f80;
       pos_hi=temp>>7;
       pos_low=angle & 0x7f;

       buff[0]=0x80;//start byte
       buff[1]=0x01;//device id
       buff[2]=0x04;//command number
       buff[3]=servo;//servo number
       buff[4]=pos_hi;//data1
       buff[5]=pos_low;//data2

       for(int i=0;i<6;i++){

          // Serial.print(buff[i],BYTE); // uses built in serial
          SWprint(buff[i]);  // uses homebrew serial
       }
    }

int servo = 0;
int value = 0;

void loop() {
	value = 0;
	servo = 0;  
	// send data only when you receive data:
	// use like s12345 where 1 is the servo number and followed by a 4 digit position, usually 500 - 5500
	if (Serial.available() > 5) {
		// read the incoming byte:
		incomingByte = Serial.read();
		if (incomingByte == 115) {  // s char
			// convert value for servo board
			servo =         (int)Serial.read() - 48; // read servo to move
			value =         ((int)Serial.read() - 48) * 1000; 
			value = value + ((int)Serial.read() - 48) * 100; 
			value = value + ((int)Serial.read() - 48) * 10; 
			value = value + ((int)Serial.read() - 48); 
			put(servo, value);
		} else if (incomingByte == 118) {  // v char
			if ((int)Serial.read() - 48 == 1) {
				digitalWrite(vibMotor, HIGH); // turn motor on
			} else {
				digitalWrite(vibMotor, LOW); // turn motor on                 
			}
			} else if (incomingByte == 99) {  // c char
				switch((int)Serial.read() - 48) {
					case 1:
					if ((int)Serial.read() - 48 == 1) {
					digitalWrite(sol1, HIGH);
					} else {
					digitalWrite(sol1, HIGH);
					}
					break;
					case 2:
					if ((int)Serial.read() - 48 == 1) {
					digitalWrite(sol2, HIGH);
					} else {
					digitalWrite(sol2, HIGH);
					}
					break;
					case 3:
					if ((int)Serial.read() - 48 == 1) {
					digitalWrite(sol3, HIGH);
					} else {
					digitalWrite(sol3, HIGH);
					}
					break;
					case 4:
					if ((int)Serial.read() - 48 == 1) {
					digitalWrite(sol4, HIGH);
					} else {
					digitalWrite(sol4, HIGH);
					}
					break;
					}  // end switch
				}
			} 
}
